#include <tbox/base/log.h>
#include <tbox/main/main.h>
#include <rclcpp/rclcpp.hpp>

using namespace std;

rclcpp::Executor::WeakPtr g_ros_exec_wptr;

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);

  LogInfo("start main");
  auto exec = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
  g_ros_exec_wptr = exec;

  //! 滤掉 --ros-args 之后的参数
  int new_argc = 0;
  for (; (new_argc < argc) &&
      strcmp(argv[new_argc], "--ros-args") &&
      strcmp(argv[new_argc], "--rclcpp-args");
      ++new_argc);

  LogInfo("start tbox");
  if (tbox::main::Start(new_argc, argv)) {
    LogInfo("start spin");
    exec->spin();
    LogInfo("spin stoped");
    tbox::main::Stop();
    LogInfo("tbox stoped");
  } else {
    LogErr("start tbox fail");
  }

  rclcpp::shutdown();
  LogInfo("main stoped");
  return 0;
}
